﻿using System.Collections;
using UnityEngine;
using System;
using System.Threading;
using System.Linq;
using System.Collections.Generic;
using System.Runtime.InteropServices;
using System.IO;
using System.Text;
using IMIForUnity;
using SimplePopup;

namespace IMIForUnity
{

    public class ImiManagerImp
    {

        private const int TIME_OUT = 20;
        private const int USER_NUM = 10;
        private const int THREAD_INTERVAL = 10;
        private const int TOTAL_JOINTS = (int)ImiSkeleton.Index.COUNT;

        private ImiManager imiManager;
        private static ImiManagerImp instance;
        private MainThreadDispatcher mainThreadDispatcher;
        private ImiAndroidHelper androidHelper;

        private ImiPreferences preferences;
        private ImiManager.OnControlPlayerChanged controlPlayerChangerObserver = null;
        private ImiManager.DepthDataProcessor depthDataProcessor = null;
        private ImiManager.ColorDataProcessor colorDataProcessor = null;
        private ImiManager.RawDataProcessor rawDataProcessor = null;



        internal enum DeviceOpenState
        {
            OPENING,
            OPENED,
            CLOSING,
            CLOSED,
        }
        //If somatosensory device is inited
        internal DeviceOpenState deviceOpenState = DeviceOpenState.CLOSED;

        /// <summary>
        /// 所有玩家体感数据存储数组.
        /// </summary>
        public Dictionary<int, ImiPlayerInfo> playerInfos;

        private ImiTextureDataHolder dataHolder;

        private Dictionary<ImiTexture2D.Type, ImiTexture2D> imiTextures;

#if UNITY_ANDROID && !UNITY_EDITOR
        private Texture2D allUserColorTexImproved;
#endif

        public ImiWrapper.ImiSkeletonFrame skeletonFrame;

        /// <summary>
        /// 是否需要超过两人的骨架处理.
        /// </summary>
        public bool isNeedMorePlayer = false;

        /// <summary>
        /// 主控玩家ID.
        /// </summary>
        private int controlPlayerId = -1;
        /// <summary>
        /// 副控玩家ID.
        /// </summary>
        private int subControlPlayerId = -2;

        private List<Vector3>[,] distanceDatas;
        private List<Vector4>[,] speedDatas;
        private List<DateTime>[,] timeDatas;

        public int speedFrameLimit = 10;

        private ErrorCode retDepth = ErrorCode.NONE;
        private ErrorCode retColor = ErrorCode.NONE;

        private bool isPrcStop = true;

        //是否手动设置主控玩家
        private bool isControlPlayerAssigned;


        //Used for getting real time
        //private DateTime epochStart;

        /// <summary>
        /// 彩色数据.
        /// </summary>
        private byte[][] queueColorDatas;
        private Queue<byte[]> colorQueue;
        private int queueNum = 0;
        private int QUEUE_SIZE = 5;

        private ushort[] depthData;

        /// <summary>
        /// 是否所有玩家检测成功.
        /// </summary>   
        private bool allPlayersCalibrated = false;

        private bool prcSkeletonLock = false;

        private static object _lock = new object();

        private ImiGestureManager imiGestureManager = ImiGestureManager.GetInstance();

        /// /////////////////////////////////////////////////////////////////////
        public Texture2D clrTex;
        public Texture2D usersLblTex;
        /// <summary>
        /// 所有玩家彩色数据.
        /// </summary>
        private byte[] usersDepthColors;
        //private byte[] colorDatas;
        /// /////////////////////////////////////////////////////////////////////


        public static ImiManagerImp GetInstance()
        {
            if (instance == null)
            {
                lock (_lock)
                {
                    if (instance == null)
                    {
                        instance = new ImiManagerImp();
                    }
                }
            }
            return instance;
        }

        private ImiManagerImp()
        {
            imiManager = ImiManager.GetInstance();
            mainThreadDispatcher = MainThreadDispatcher.GetInstance();

        }


        internal void Update()
        {
            if (deviceOpenState == DeviceOpenState.OPENED)
            {
                //Debug.Log("frameId = " + frameIdQueue.Dequeue());
                if (colorQueue.Count > 0)
                {
                    clrTex.LoadRawTextureData(colorQueue.Dequeue());
                    clrTex.Apply();
                }
                if (retDepth == ErrorCode.OK)
                {
                    UpdateDepthTex();
                }
            }
            else
            {
                //Debug.Log("Buffer Empty!");
            }

            //if (imiTextures != null && dataHolder != null)
            //{
            //    if(colorQueue != null && colorQueue.Count > 0)
            //    {
            //        dataHolder.setColorDataRaw(colorQueue.Dequeue());
            //    }
            //    List<ImiTexture2D.Type> keys = new List<ImiTexture2D.Type>(imiTextures.Keys);
            //    for(int i = 0; i<keys.Count; i++)
            //    {
            //        imiTextures[keys[i]].UpdateTexture();
            //    }
            //    //telling the holder to prepare next frame
            //    dataHolder.SetTextureUsed();
            //}

#if UNITY_ANDROID && !UNITY_EDITOR
            if (deviceOpenState == DeviceOpenState.OPENED && imiManager.IsPerformance)
            {
                if (preferences.ColorStream)
                {
                    IssuePluginEvent(MediaSurfaceEventType.Update);
                }
            }
#endif
        }

        /// <summary>s
        /// Init Device depending on platform
        /// </summary>
        public void Open()
        {

#if UNITY_ANDROID && !UNITY_EDITOR
            if (imiManager.useRootPermission)
            {
                realInit();
            }
            else
            {
                if(deviceOpenState == DeviceOpenState.CLOSED)
                {
                    Log.d("Android Platform");
                    UseAndroidPermission();
                }else
                {
                    Log.d("Device Is Opening, Don't call again");
                }
            }
#else
            realInit();
#endif
        }

        private bool isGetttingPermission = false;
        public void UseAndroidPermission()
        {
            if (isGetttingPermission)
            {
                return;
            }
            isGetttingPermission = true;
            androidHelper = ImiAndroidHelper.GetInstance();
            androidHelper.RequirePermission(delegate (ErrorCode code)
            {
                if (code == ErrorCode.OK)
                {
                    Log.d("Permission Granted, Trying to initialize device");
                    realInit();
                }
                else
                {
                    Log.e("Device Inited failed! Maybe permission is denied!");
                    Log.e("We can try again!");
                    realInit();
                }
            });
        }

        static void UnityLog(string log)
        {
            Debug.Log("Native: " + log);
        }

        private void OnUpgradeDownloadedImpl()
        {
            Debug.Log("Update.zip Downloaded! Start Upgrading...");
            mainThreadDispatcher.Enqueue(ShowPopup);
        }

        private void OnDeviceUpgradingImpl(ImiWrapper.IMI_UPGRADE_PROMPT promptId, float progress)
        {
            mainThreadDispatcher.Enqueue(() => { showUpgradeProgress(promptId, progress); });

        }

        private void showUpgradeProgress(ImiWrapper.IMI_UPGRADE_PROMPT promptId, float progress)
        {
            if (SimplePopupManager.Instance != null)
            {
                SimplePopupManager.Instance.SetProgress(progress);
            }
            if (promptId == ImiWrapper.IMI_UPGRADE_PROMPT.IMI_UPGRADE_PASS)
            {
                OnUpdateFinished();
            }
            Debug.Log("Device Upgrading...PromptId = " + promptId + " progress = " + progress);
        }

        private void InitUpgradeCallback()
        {

            ImiWrapper.OnUpgradeDownloaded downloadedCallback = new ImiWrapper.OnUpgradeDownloaded(OnUpgradeDownloadedImpl);
            System.IntPtr downloadedPtr = Marshal.GetFunctionPointerForDelegate(downloadedCallback);

            ImiWrapper.OnDeviceUpgrading upgradingCallback = new ImiWrapper.OnDeviceUpgrading(OnDeviceUpgradingImpl);
            System.IntPtr upgradingPtr = Marshal.GetFunctionPointerForDelegate(upgradingCallback);
            ImiWrapper.SetDeviceUpgradeCallback(downloadedPtr, upgradingPtr);
            Debug.Log("Calling Upgrade!");

        }

        private void InitLogCallback()
        {
            ImiWrapper.UnityLog logCallback = new ImiWrapper.UnityLog(UnityLog);
            System.IntPtr logPtr = Marshal.GetFunctionPointerForDelegate(logCallback);
            ImiWrapper.SetLogFunction(logPtr);
        }

        public void OnUpdateFinished()
        {
            SimplePopupManager.Instance.CreatePopup("升级完成，请重新打开游戏！");
            SimplePopupManager.Instance.AddButton("退出", delegate { Close(); Application.Quit(); });
            SimplePopupManager.Instance.ShowPopup();
        }


        public void ShowPopup()
        {
            SimplePopupManager.Instance.CreatePopup("系统检测到固件更新，是否升级？");
            SimplePopupManager.Instance.AddButton("Yes", delegate { ImiWrapper.SetUpgrade(true); ShowProgress(); });
            SimplePopupManager.Instance.AddButton("No", delegate { Debug.Log("clicked on no"); });
            SimplePopupManager.Instance.ShowPopup();
        }

        public void ShowProgress()
        {
            SimplePopupManager.Instance.CreatePopup("正在升级中，请不要拔掉电源...");
            SimplePopupManager.Instance.EnableProgressBar(true);
            SimplePopupManager.Instance.SetProgress(0);
            SimplePopupManager.Instance.ShowPopup();
        }

#if (UNITY_ANDROID && !UNITY_EDITOR)
        private enum MediaSurfaceEventType
        {
            Initialize = 0,
            Shutdown = 1,
            Update = 2,
            Max_EventType
        };

        public static int eventBase
        {
            get { return _eventBase; }
            set
            {
                _eventBase = value;
                if (ImiManager.GetInstance() != null)
                {
                    if (ImiManager.GetInstance().IsPerformance)
                    {
			            ImiWrapper.OVR_Media_Surface_SetEventBase(_eventBase);
                    }
                }
            }
        }
        private static int _eventBase = 0;

        private static void IssuePluginEvent(MediaSurfaceEventType eventType)
        {
#pragma warning disable CS0618 // Type or member is obsolete
            GL.IssuePluginEvent((int)eventType + eventBase);
#pragma warning restore CS0618 // Type or member is obsolete
        }


#endif
        private void realInit()
        {
            //If already inited, return
            if (deviceOpenState != DeviceOpenState.CLOSED)
            {
                Debug.Log("Device is not Closed now, it might be already opened, or you are opening/closing it");
                return;
            }

            InitPreferences();

#if UNITY_ANDROID && !UNITY_EDITOR
            if (imiManager.IsPerformance)
            {
                if (preferences.ColorStream)
                {
                    IssuePluginEvent(MediaSurfaceEventType.Initialize);

                    allUserColorTexImproved = Texture2D.CreateExternalTexture(preferences.colorResolution.Width, preferences.colorResolution.Height, TextureFormat.RGB24, false, false, System.IntPtr.Zero);
                    allUserColorTexImproved.filterMode = FilterMode.Point;
                    allUserColorTexImproved.Apply();

                    ImiWrapper.Imi_Renderer_Surface(allUserColorTexImproved.GetNativeTexturePtr(), preferences.colorResolution.Width, preferences.colorResolution.Height);
                }
            }
#endif

            //If upgrade the firmware and drivers(not unity sdk)
            if (imiManager.AutoUpgrade)
                InitUpgradeCallback();

            InitLogCallback();

            //Step One : Open Device and streams in sub thread
            //Step Two will be called afterwards in sub thread
            Thread openDeviceThread = new Thread(new ThreadStart(OpenStepOne));
            openDeviceThread.Start();

        }

        //This method should be used in sub thread to avoid blocking the main thread
        private void OpenStepOne()
        {
            if (deviceOpenState == DeviceOpenState.CLOSED)
                if (IminectInitialize())
                {
                    mainThreadDispatcher.Enqueue(OpenStepTwo);
                }
                else
                {
                    Log.e("Open Device Failed! Closing...");
                    mainThreadDispatcher.Enqueue(Close);
                }
        }

        //When the device is opened, init other resources
        private void OpenStepTwo()
        {
            InitializeDisplayImageData();
            InitializeIminectData();

            InitPrcImiDataThd();

            deviceOpenState = DeviceOpenState.OPENED;
            Log.d("Device is Opened!");
            imiManager._OnDeviceOpened();
        }

        public void Close()
        {

            deviceOpenState = DeviceOpenState.CLOSING;
            isPrcStop = true;

            //Close should not be called in sub thread
            //Otherwise quitting the application cannot 
            //close device successfully
            CloseStepOne();
            CloseStepTwo();

#if UNITY_ANDROID && !UNITY_EDITOR
            if (preferences.ColorStream)
            {
                IssuePluginEvent(MediaSurfaceEventType.Shutdown);
            }
#endif

            deviceOpenState = DeviceOpenState.CLOSED;
            imiManager._OnDeviceClosed();
            Debug.Log("Device Is Closed");

        }

        private void CloseStepOne()
        {
            ErrorCode error;

            if (preferences.DepthStream)
            {
                error = ImiWrapper.CloseStream(ImiWrapper.ImiFrameType.IMI_DEPTH_FRAME);
                if (error != ErrorCode.OK)
                {
                    Log.e(error);
                }
            }

            if (preferences.SkeletonStream)
            {
                error = ImiWrapper.CloseStream(ImiWrapper.ImiFrameType.IMI_SKELETON_FRAME);
                if (error != ErrorCode.OK)
                {
                    Log.e(error);
                }
            }

            if (preferences.DepthSkeletonStream)
            {
                error = ImiWrapper.CloseStream(ImiWrapper.ImiFrameType.IMI_DEPTH_SKELETON_FRAME);
                if (error != ErrorCode.OK)
                {
                    Log.e(error);
                }
            }

            if (preferences.UserDepthAndSkeletonStream)
            {
                error = ImiWrapper.CloseStream(ImiWrapper.ImiFrameType.IMI_USER_INDEX_SKELETON_FRAME);
                if (error != ErrorCode.OK)
                {
                    Log.e(error);
                }
            }

            if (preferences.ColorStream)
            {
                error = ImiWrapper.CloseStream(ImiWrapper.ImiFrameType.IMI_COLOR_FRAME);
                if (error != ErrorCode.OK)
                {
                    Log.e(error);
                }
            }

            error = ImiWrapper.CloseDevice();
            if (error != ErrorCode.OK)
            {
                Log.e(error);
            }

        }

        private void CloseStepTwo()
        {
#if UNITY_ANDROID && !UNITY_EDITOR
            if (androidHelper != null)
                androidHelper.shutdown();
#endif

            ReleaseIminectData();
            ReleaseDisplayImageData();

            //Tell GC to collect memory
            GC.Collect();
            GC.WaitForPendingFinalizers();


        }

        private void InitPrcImiDataThd()
        {
            if (deviceOpenState != DeviceOpenState.OPENING)
            {
                Debug.Log("Device is in opening state, so don't open process threads");
                return;
            }
            if (!isPrcStop)
            {
                Debug.Log("These Threads might have already been opened!");
            }

            isPrcStop = false;

            if (preferences.DepthSkeletonStream || preferences.SkeletonStream || preferences.DepthStream || preferences.UserDepthAndSkeletonStream)
            {
                Thread prcDSThd = new Thread(new ThreadStart(PrcDepthData));
                prcDSThd.Start();
            }

            if (preferences.ColorStream)
            {
                Thread prcColorThd = new Thread(new ThreadStart(PrcColorData));
                prcColorThd.Start();
            }

            Debug.Log("Thread Started!");
        }

        private ImiResolution depthResolution;
        private ImiResolution colorResolution;

        /// <summary>
        /// Init user defined properties
        /// </summary>
        private void InitPreferences()
        {
            if (imiManager.useRootPermission && imiManager.IsPerformance)
            {
                Debug.LogError("Performance Mode Cannot use root permission, disable performance");
                imiManager.IsPerformance = false;
            }
            preferences = ImiPreferences.GetInstance();
            preferences.ColorStream = false;
            preferences.DepthSkeletonStream = false;
            preferences.UserDepthAndSkeletonStream = false;
            preferences.DepthStream = false;
            preferences.SkeletonStream = false;

            if (imiManager.ColorTexture)
            {
                preferences.ColorStream = true;
            }

            if (imiManager.UserDepthTexture)
            {
                preferences.UserDepthAndSkeletonStream = true;
            }
            else if (imiManager.DepthTexture && imiManager.SkeletonData)
            {
                preferences.DepthSkeletonStream = true;
            }
            else if (imiManager.DepthTexture)
            {
                preferences.DepthStream = true;
            }
            else if (imiManager.SkeletonData)
            {
                preferences.SkeletonStream = true;
            }

            if (!imiManager.IsPerformance && imiManager.imageResolution == ImiResolution.Type._1920x1080)
            {
                Debug.LogError("1920x1080 resolution cannot be used without performance enabled");
                imiManager.imageResolution = ImiResolution.Type._1280x720;
            }
            if (imiManager.depthResolution != ImiResolution.Type._640x480)
            {
                Debug.LogError("DepthResolution only supports 640x480 now");
                imiManager.depthResolution = ImiResolution.Type._640x480;
            }

            preferences.colorResolution.type = imiManager.imageResolution;
            preferences.depthResolution.type = imiManager.depthResolution;

            depthResolution = preferences.depthResolution;
            colorResolution = preferences.colorResolution;

        }


        //绘制相关数据初始化.
        private void InitializeDisplayImageData()
        {
            // Initialize depth map stuff
            depthData = new ushort[preferences.depthResolution.Size];

            colorQueue = new Queue<byte[]>();
            queueColorDatas = new byte[QUEUE_SIZE][];
            for (int i = 0; i < QUEUE_SIZE; i++)
            {
                queueColorDatas[i] = new byte[preferences.colorResolution.Size * 3];
            }

            //dataHolder = ImiTextureDataHolder.GetInstance();
            //dataHolder.InitHolder();
            //imiTextures = new Dictionary<ImiTexture2D.Type, ImiTexture2D>();


            clrTex = new Texture2D(640, 480, TextureFormat.RGB24, false);
            clrTex.filterMode = FilterMode.Point;
            usersLblTex = new Texture2D(640, 480, TextureFormat.RGBA32, false);
            usersDepthColors = new byte[preferences.depthResolution.Size * 4];
        }

        private void ReleaseDisplayImageData()
        {
            if (imiTextures != null)
            {
                List<ImiTexture2D.Type> keys = new List<ImiTexture2D.Type>(imiTextures.Keys);
                foreach (ImiTexture2D.Type key in keys)
                {
                    imiTextures[key].ReleaseTexture();
                }
                imiTextures.Clear();
            }
            imiTextures = null;
            if (dataHolder != null)
            {
                dataHolder.ReleaseHolder();
                dataHolder = null;
            }

            depthData = null;
            //if(colorQueue != null)
            //{
            //    colorQueue.Clear();
            //    colorQueue = null;
            //}
            //queueColorDatas = null;
        }


        /// <summary>
        /// Initialize somatosensor data
        /// </summary>
        private void InitializeIminectData()
        {
            playerInfos = new Dictionary<int, ImiPlayerInfo>();

            distanceDatas = new List<Vector3>[USER_NUM, TOTAL_JOINTS];
            speedDatas = new List<Vector4>[USER_NUM, TOTAL_JOINTS];
            timeDatas = new List<DateTime>[USER_NUM, TOTAL_JOINTS];
            for (int i = 0; i < USER_NUM; i++)
            {
                for (int j = 0; j < TOTAL_JOINTS; j++)
                {
                    distanceDatas[i, j] = new List<Vector3>();
                    speedDatas[i, j] = new List<Vector4>();
                    timeDatas[i, j] = new List<DateTime>();
                }
            }
            //epochStart = new System.DateTime(1970, 1, 1, 8, 0, 0, System.DateTimeKind.Utc);
        }

        private void ReleaseIminectData()
        {
            if (playerInfos != null)
            {
                playerInfos.Clear();
                for (int i = 0; i < USER_NUM; i++)
                {
                    for (int j = 0; j < TOTAL_JOINTS; j++)
                    {
                        distanceDatas[i, j].Clear();
                        distanceDatas[i, j] = null;
                        speedDatas[i, j].Clear();
                        speedDatas[i, j] = null;
                        timeDatas[i, j].Clear();
                        timeDatas[i, j] = null;
                    }
                }
                distanceDatas = null;
                speedDatas = null;
                timeDatas = null;
                playerInfos = null;
            }
        }


        /// <summary>
        /// Open Device and Open Streams according to the user choice
        /// </summary>
        private bool IminectInitialize()
        {
            //Returns if Device is unavailable
            //Returns if Already Initialized
            if (!IsDeviceAvailable() || deviceOpenState != DeviceOpenState.CLOSED)
            {
                return false;
            }
            deviceOpenState = DeviceOpenState.OPENING;

            ErrorCode error;
            if ((error = OpenDevice()) != ErrorCode.OK)
            {
                //Log.e(error);
                Debug.Log("Open Device Failed, error is = " + error);
                return false;
            }

            //Open Depth/Skeleton Stream
            error = openDepthAndSkeletonStream();

            if (error != ErrorCode.OK)
            {
                Log.e(error);
                return false;
            }

            //Open Color Stream
            if (preferences.ColorStream)
            {

#if UNITY_ANDROID && !UNITY_EDITOR
                if (imiManager.IsPerformance)
                {
                    if ((error = ImiWrapper.OpenStream(ImiWrapper.ImiFrameType.IMI_COLOR_FRAME,
                        preferences.colorResolution.type,
                        ImiWrapper.ImiPixelFormat.IMI_PIXEL_FORMAT_IMAGE_H264))
                    != ErrorCode.OK)
                    {
                        Log.e(error);
                        return false;
                    }
                }else
                {
                    if ((error = ImiWrapper.OpenStream(ImiWrapper.ImiFrameType.IMI_COLOR_FRAME,
                        preferences.colorResolution.type,
                        ImiWrapper.ImiPixelFormat.IMI_PIXEL_FORMAT_IMAGE_RGB24))
                    != ErrorCode.OK)
                    {
                        Log.e(error);
                        return false;
                    }
                }
#else
                if ((error = ImiWrapper.OpenStream(ImiWrapper.ImiFrameType.IMI_COLOR_FRAME,
                    preferences.colorResolution.type,
                    ImiWrapper.ImiPixelFormat.IMI_PIXEL_FORMAT_IMAGE_RGB24))
                != ErrorCode.OK)
                {
                    Log.e(error);
                    return false;
                }
#endif
                Debug.Log("Open Color Stream Ok!");
            }

            return true;

        }

        /// <summary>
        /// This method open depth stream and/or skeleton streams properly
        /// User can only select one in the four : depth, skeleton, depth And skeleton, user depth and skeleton
        /// </summary>
        private ErrorCode openDepthAndSkeletonStream()
        {
            ImiWrapper.ImiFrameType streamType;

            if (preferences.UserDepthAndSkeletonStream)
            {
                streamType = ImiWrapper.ImiFrameType.IMI_USER_INDEX_SKELETON_FRAME;
            }
            else if (preferences.DepthSkeletonStream)
            {
                streamType = ImiWrapper.ImiFrameType.IMI_DEPTH_SKELETON_FRAME;
            }
            else if (preferences.DepthStream)
            {
                streamType = ImiWrapper.ImiFrameType.IMI_DEPTH_FRAME;
            }
            else if (preferences.SkeletonStream)
            {
                streamType = ImiWrapper.ImiFrameType.IMI_SKELETON_FRAME;
            }
            else
            {
                //Don't open any kind of depth stream or skeleton stream
                return ErrorCode.OK;
            }
            Debug.Log("Opening  " + streamType);
            return ImiWrapper.OpenStream(streamType,
                preferences.depthResolution.type,
                ImiWrapper.ImiPixelFormat.IMI_PIXEL_FORMAT_DEP_16BIT);
        }

        private bool showDepthFrame()
        {
            return preferences.SkeletonStream || preferences.DepthStream || preferences.UserDepthAndSkeletonStream || preferences.DepthSkeletonStream;
        }

        private void PrcColorData()
        {
            while (!isPrcStop)
            {
                if (deviceOpenState == DeviceOpenState.OPENED)
                {
#if UNITY_ANDROID && !UNITY_EDITOR
                    if (imiManager.IsPerformance)
                    {
                        ImiWrapper.LoadColorTexture(ImiWrapper.ImiPixelFormat.IMI_PIXEL_FORMAT_IMAGE_H264);//H264
                        Thread.Sleep(THREAD_INTERVAL);
                    }else{
#endif
                    if (colorQueue.Count == QUEUE_SIZE)
                    {
                        continue;
                    }
                    else
                    {
                        retColor = ImiWrapper.GetColorData(queueColorDatas[queueNum], TIME_OUT);

                        if (retColor == ErrorCode.OK)
                        {
                            colorQueue.Enqueue(queueColorDatas[queueNum]);
                            queueNum++;
                        }

                        if (queueNum == QUEUE_SIZE)
                        {
                            queueNum = 0;
                        }
                    }
#if UNITY_ANDROID && !UNITY_EDITOR
                    }
#endif

                }
            }
        }

        //体感数据处理.
        private void PrcDepthData()
        {
            while (!isPrcStop)
            {
                if (deviceOpenState == DeviceOpenState.OPENED)
                {
                    if (preferences.DepthSkeletonStream)
                    {
                        retDepth = ImiWrapper.GetDepthDataAndSkeletonFrame(depthData, ref skeletonFrame, TIME_OUT);
                    }
                    else if (preferences.DepthStream)
                    {
                        retDepth = ImiWrapper.GetDepthData(depthData, TIME_OUT);
                    }
                    else if (preferences.SkeletonStream)
                    {
                        retDepth = ImiWrapper.GetSkeletonFrame(ref skeletonFrame, TIME_OUT);
                    }
                    else if (preferences.UserDepthAndSkeletonStream)
                    {
                        //System.Diagnostics.Stopwatch watch = System.Diagnostics.Stopwatch.StartNew();
                        retDepth = ImiWrapper.GetDepthPlayerDataAndSkeletonFrame(depthData, ref skeletonFrame, TIME_OUT);
                        //if (watch.ElapsedMilliseconds > 5)
                        //{
                        //    //Log.d("reading time: " + watch.ElapsedMilliseconds);
                        //}
                    }

                    if (retDepth == ErrorCode.OK)
                    {
                        if (preferences.SkeletonStream || preferences.UserDepthAndSkeletonStream || preferences.DepthSkeletonStream)
                        {
                            ProcessSkeleton();
                            imiGestureManager.Update();
                        }

                        if (depthDataProcessor != null)
                        {
                            depthDataProcessor(depthData);
                        }
                        else
                        {
                            if (preferences.DepthSkeletonStream || preferences.UserDepthAndSkeletonStream || preferences.DepthStream)
                            {
                                if (!prcDepthDrawLock)
                                {
                                    UpdateUserDepthDrawData();
                                    prcDepthDrawLock = true;
                                }
                                //bool processed = false;
                                //while (!processed)
                                //{
                                //    processed = dataHolder.ProcessRawData(depthData, imiManager.ShowDepthDetail);
                                //}
                            }
                        }
                    }
                    else
                    {
                        //Debug.Log("Reading Depth Data Failed");
                    }
                }
            }
        }

        public ErrorCode OpenDevice()
        {

#if UNITY_ANDROID && !UNITY_EDITOR
            ErrorCode error;
            if (imiManager.useRootPermission)
            {
                //open device on rooted system
                if ((error = ImiWrapper.OpenDevice()) != ErrorCode.OK)
                {
                    Log.e(error);
                    return ErrorCode.OPEN_DEVICE_FAILED;
                }
                return ErrorCode.OK;
            }
            //return if the device has not been opened
            if (androidHelper.fd == -1)
            {
                return ErrorCode.OPEN_DEVICE_FAILED;
            }

            if (androidHelper.fd == 0)
            {
                //open device on rooted system
                if ((error = ImiWrapper.OpenDevice()) != ErrorCode.OK)
                {
                    Log.e(error);
                    return ErrorCode.OPEN_DEVICE_FAILED;
                }
            }
            else
            {
                if ((error = ImiWrapper.OpenDevice2(androidHelper.fd, androidHelper.usbPath)) != ErrorCode.OK)
                {
                    Log.e(error);
                    return ErrorCode.OPEN_DEVICE_FAILED;
                }
            }
#else
            //open device on rooted system
            if (ImiWrapper.OpenDevice() != ErrorCode.OK)
            {
                return ErrorCode.OPEN_DEVICE_FAILED;
            }
#endif
            return ErrorCode.OK;
        }


        public bool IsDeviceAvailable()
        {
#if UNITY_ANDROID && !UNITY_EDITOR
            if (imiManager.useRootPermission)
            {
                return true;
            }else
            {
                return androidHelper.fd != -1;
            }
#else
            return true;
#endif
        }


        private ImiWrapper.ImiDeviceState deviceState = ImiWrapper.ImiDeviceState.IMI_DEVICE_STATE_DISCONNECT;
        /**
         * TODO support for hot swap
         */
        public void CheckDeviceState()
        {
            ImiWrapper.ImiDeviceState newState = ImiWrapper.GetDeviceState();
            if (newState == deviceState)
            {
                return;
            }
            deviceState = newState;
            if (deviceState == ImiWrapper.ImiDeviceState.IMI_DEVICE_STATE_DISCONNECT)
            {
                Log.e("IMI_DEVICE_STATE_DISCONNECT");
                imiManager._OnDeviceDisconnected();
                Close();
            }

            if (deviceState == ImiWrapper.ImiDeviceState.IMI_DEVICE_STATE_CONNECT)
            {
                Log.d("IMI_DEVICE_STATE_CONNECT");
                imiManager._OnDeviceConnected();
                OpenDevice();
            }
        }


        //前景图绘制数据处理.
        void UpdateUserDepthDrawData()
        {
            Array.Clear(usersDepthColors, 0, usersDepthColors.Length);
            int tempPlayerIndex;
            int userDepth;
            for (int i = 0; i < preferences.depthResolution.Size; i++)
            {
                 tempPlayerIndex = depthData[i] & 7;
                if (tempPlayerIndex == 0)
                    continue;
                userDepth = depthData[i] >> 3;
                if(tempPlayerIndex == controlPlayerId)
                {
                    usersDepthColors[i * 4] = 255;
                    usersDepthColors[i * 4 + 1] = 105;
                    usersDepthColors[i * 4 + 2] = 180;
                    usersDepthColors[i * 4 + 3] = 255;
                }
                else if(tempPlayerIndex == subControlPlayerId)
                {
                    usersDepthColors[i * 4] = 255;
                    usersDepthColors[i * 4 + 1] = 215;
                    usersDepthColors[i * 4 + 2] = 0;
                    usersDepthColors[i * 4 + 3] = 255;
                }
                else
                {
                    usersDepthColors[i * 4] = 125;
                    usersDepthColors[i * 4 + 1] = 125;
                    usersDepthColors[i * 4 + 2] = 125;
                    usersDepthColors[i * 4 + 3] = 255;
                }
            }
        }
        private bool prcDepthDrawLock = false;

        private void UpdateDepthTex()
        {
            if (prcDepthDrawLock)
            {
                usersLblTex.LoadRawTextureData(usersDepthColors);
                usersLblTex.Apply();

                prcDepthDrawLock = false;
            }
        }

        //Remove users who entered before and now no longer been tracked
        //private void removeUnTrackedUser()
        //{
        //    List<int> keys = new List<int>(playerInfos.Keys);
        //    for (int i = 0; i < keys.Count; i++)
        //    {
        //        if (keys[i] != skeletonFrame.skeletonData[0].usrIndex
        //            && keys[i] != skeletonFrame.skeletonData[1].usrIndex)
        //        {
        //            playerInfos.Remove(keys[i]);
        //            if (keys[i] == controlPlayerId)
        //            {
        //                //主控玩家丢失
        //                isControlPlayerAssigned = false;
        //            }
        //        }
        //    }

        //}

        /// <summary>
        /// 更新主控玩家和次控玩家
        /// </summary>
        private void updateAllControlPlayerId()
        {
            int oldControlId = controlPlayerId;
            subControlPlayerId = -2;
            //分配主要玩家
            if (!isControlPlayerAssigned)
            {
                //如果没有被指定，选择最小的id作为主要玩家
                controlPlayerId = 100;
                for (int i = 0; i <ImiWrapper.MAX_TRACKED_PEOPLE_NUM; i++)
                {
                    ImiWrapper.ImiSkeletonData skeletonData = skeletonFrame.skeletonData[i];
                    //Debug.Log("Player " + i + " Tracking State = " + skeletonFrame.skeletonData[i].trackingState);
                    if (skeletonFrame.skeletonData[i].trackingState == ImiSkeletonTrackingState.IMI_SKELETON_TRACKED)
                    {
                        if (skeletonData.usrIndex > 0 && skeletonData.usrIndex < controlPlayerId)
                        {
                            controlPlayerId = (int)skeletonData.usrIndex;
                        }
                    }
                }
            }
            if (controlPlayerId != oldControlId && controlPlayerChangerObserver != null)
            {
                controlPlayerChangerObserver(oldControlId, controlPlayerId);
            }

            //分配次要玩家：除了主控玩家的另一个玩家，是次控玩家
            for (int i = 0; i < ImiWrapper.MAX_TRACKED_PEOPLE_NUM; i++)
            {
                ImiWrapper.ImiSkeletonData skeletonData = skeletonFrame.skeletonData[i];
                if (skeletonFrame.skeletonData[i].trackingState == ImiSkeletonTrackingState.IMI_SKELETON_TRACKED)
                {
                    if (skeletonData.usrIndex != controlPlayerId && skeletonData.usrIndex > 0)
                    {
                        subControlPlayerId = (int)skeletonData.usrIndex;
                    }
                }
            }
        }

        //骨架数据处理.
        private bool ProcessSkeleton()
        {
            if (prcSkeletonLock)
            {
                return false;
            }

            prcSkeletonLock = true;
            if (skeletonFrame.skeletonData == null)
            {
                prcSkeletonLock = false;
                return true;
            }
            //removeUnTrackedUser();
            updateAllControlPlayerId();
            //process every skeleton in the frame
            for (int i = 1; i <= ImiWrapper.MAX_TRACKED_PEOPLE_NUM; i++)
            {
                ImiWrapper.ImiSkeletonData skeletonData = skeletonFrame.skeletonData[i-1];
                //process the tracked skeleton

                //Debug.Log("Player id = " + i + "   "+ skeletonData.trackingState);
                ImiPlayerInfo playerInfo = null;
                if(!playerInfos.TryGetValue((int)skeletonData.trackingID,out playerInfo))
                {
                    playerInfo = new ImiPlayerInfo();
                    playerInfo.SetUserId((int)skeletonData.trackingID);
                    playerInfos.Add((int)skeletonData.trackingID,playerInfo);
                    Debug.Log("New Player Added, player index is " + playerInfo.userId);
                }
                if (skeletonData.trackingState != ImiSkeletonTrackingState.IMI_SKELETON_TRACKED)
                {
                    playerInfo.SetPlayerTracked(false);
                    continue;
                }
                playerInfo.SetPlayerTracked(true);

                int stateNotTracked = (int)ImiSkeletonPositionTrackingState.IMI_SKELETON_POSITION_NOT_TRACKED;

                playerInfo.SetPlayerPosition(skeletonData.position.x, skeletonData.position.y, skeletonData.position.z);

                for (int j = 0; j < TOTAL_JOINTS; j++)
                {
                    playerInfo.GetSkeletons()[j].isTracked = ((int)skeletonData.skeletonPositionTrackingStates[j] != stateNotTracked);
                    playerInfo.GetSkeletons()[j].position.x = skeletonData.skeletonPositions[j].x;
                    playerInfo.GetSkeletons()[j].position.y = skeletonData.skeletonPositions[j].y;
                    playerInfo.GetSkeletons()[j].position.z = skeletonData.skeletonPositions[j].z;
                    playerInfo.GetSkeletons()[j].positionV4 = skeletonData.skeletonPositions[j];
                }
                UpdateGesture(playerInfo);
                CalcLimbsAngular(playerInfo);

                if (preferences.isNeedJointsSpeed)
                {
                    CalcJointsSpeed(playerInfo);
                }

                if (preferences.isNeedJointsOri)
                {
                    CalcJointsOri(playerInfo);
                }

                FixNeedPos(playerInfo);

            }
            prcSkeletonLock = false;
            return true;
        }

        private void UpdateGesture(ImiPlayerInfo player)
        {
            //bool[] jointsTracked = player.GetPlayerJointsTracked();
            //InternalType.SkeletonJointState[] jointStates = new InternalType.SkeletonJointState[(int)ImiSkeleton.Index.COUNT];
            //Vector3[] positions = player.GetPlayerJointsPos();
            //float[] confidences = new float[(int)ImiSkeleton.Index.COUNT];

            //InternalType.Point3f[] jointPositions = new InternalType.Point3f[(int)ImiSkeleton.Index.COUNT];

            //for (int i = 0; i < (int)ImiSkeleton.Index.COUNT; i++)
            //{
            //    jointPositions[i].x = positions[i].x * 1000;
            //    jointPositions[i].y = positions[i].y * 1000;
            //    jointPositions[i].z = positions[i].z * 1000;

            //    confidences[i] = 1;
            //    if (jointsTracked[i])
            //        jointStates[i] = InternalType.SkeletonJointState.SKELETON_JOINT_TRACKED;
            //    else
            //        jointStates[i] = InternalType.SkeletonJointState.SKELETON_JOINT_NOT_TRACKED;
            //}
            //InternalType.ImiSkeletonData skeletonData = new InternalType.ImiSkeletonData();
            //skeletonData.m_trackingID = (uint)player.GetUserId();
            //skeletonData.m_state = InternalType.SkeletonState.SKELETON_TRACKED;
            //skeletonData.points = jointPositions;
            //skeletonData.m_jointPositionConfidences = confidences;
            //skeletonData.skeletonJointStates = jointStates;
            //skeletonData.jointTypes = imiManager.joints;
            ////Log.d("当前时间毫秒数:" + (DateTime.Now - baseTime).TotalMilliseconds);
            //int updatere = ImiGestureWrapper.ImiUpdateGesture((uint)player.GetUserId(), ref skeletonData);
            ////Log.d("update return value " + updatere);
        }
        //计算玩家站直身体hipcenter高度.
        private void FixNeedPos(ImiPlayerInfo controlPlayerInfo)
        {
            if (controlPlayerInfo.GetPlayerPosition().z > 1.2f && !controlPlayerInfo.isOrigFixRec)
            {
                if (controlPlayerInfo.GetSkeletons()[ImiGesture.leftHipIndex].isTracked &&
                    controlPlayerInfo.GetSkeletons()[ImiGesture.leftKneeIndex].isTracked &&
                    controlPlayerInfo.GetSkeletons()[ImiGesture.rightHipIndex].isTracked &&
                    controlPlayerInfo.GetSkeletons()[ImiGesture.rightKneeIndex].isTracked &&
                    Math.Abs(controlPlayerInfo.GetSkeletons()[ImiGesture.leftHipIndex].position.z -
                    controlPlayerInfo.GetSkeletons()[ImiGesture.rightHipIndex].position.z) < 0.04f &&
                    (controlPlayerInfo.GetPlayerLimbsAngular()[ImiGesture.leftThighIndex].y < 0 &&
                    controlPlayerInfo.GetPlayerLimbsAngular()[ImiGesture.rightThighIndex].y < 0) ||
                    (Math.Abs(controlPlayerInfo.GetPlayerLimbsAngular()[ImiGesture.leftThighIndex].y) > 83.0f &&
                    Math.Abs(controlPlayerInfo.GetPlayerLimbsAngular()[ImiGesture.rightThighIndex].y) > 83.0f))
                {
                    ImiGesture.fixPosZ = controlPlayerInfo.GetPlayerPosition().z;
                    ImiGesture.fixHipPosY = controlPlayerInfo.GetSkeletons()[ImiGesture.hipCenterIndex].position.y;
                    controlPlayerInfo.isOrigFixRec = true;
                }
            }
            else if (controlPlayerInfo.isOrigFixRec)
            {
                if (Math.Abs(controlPlayerInfo.GetPlayerPosition().z - ImiGesture.fixPosZ) > 0.2f)
                {
                    if (controlPlayerInfo.GetSkeletons()[ImiGesture.leftHipIndex].isTracked &&
                        controlPlayerInfo.GetSkeletons()[ImiGesture.leftKneeIndex].isTracked &&
                        controlPlayerInfo.GetSkeletons()[ImiGesture.rightHipIndex].isTracked &&
                        controlPlayerInfo.GetSkeletons()[ImiGesture.rightKneeIndex].isTracked &&
                        Math.Abs(controlPlayerInfo.GetSkeletons()[ImiGesture.leftHipIndex].position.z -
                        controlPlayerInfo.GetSkeletons()[ImiGesture.rightHipIndex].position.z) < 0.04f &&
                        (controlPlayerInfo.GetPlayerLimbsAngular()[ImiGesture.leftThighIndex].y < 0 &&
                        controlPlayerInfo.GetPlayerLimbsAngular()[ImiGesture.rightThighIndex].y < 0) ||
                        (Math.Abs(controlPlayerInfo.GetPlayerLimbsAngular()[ImiGesture.leftThighIndex].y) > 83.0f &&
                        Math.Abs(controlPlayerInfo.GetPlayerLimbsAngular()[ImiGesture.rightThighIndex].y) > 83.0f))
                    {
                        ImiGesture.fixPosZ = controlPlayerInfo.GetPlayerPosition().z;
                        ImiGesture.fixHipPosY = controlPlayerInfo.GetSkeletons()[ImiGesture.hipCenterIndex].position.y;
                    }
                }
            }
        }


        //计算肢体角度.
        private void CalcLimbsAngular(ImiPlayerInfo playerInfo)
        {
            for (int j = 0; j < (int)Limb.Count; j++)
            {
                Vector3 limbAng = new Vector3(-1.0f, -1.0f, -1.0f);
                switch ((Limb)Enum.ToObject(typeof(Limb), j))
                {
                    case Limb.Left_Forearm:
                        limbAng = CalcLimbAngular(playerInfo, ImiGesture.leftHandIndex, ImiGesture.leftElbowIndex);
                        break;
                    case Limb.Left_Forearm_Wrist:
                        limbAng = CalcLimbAngular(playerInfo, ImiGesture.leftWristIndex, ImiGesture.leftElbowIndex);
                        break;
                    case Limb.Left_Postbrachium:
                        limbAng = CalcLimbAngular(playerInfo, ImiGesture.leftElbowIndex, ImiGesture.leftShoulderIndex);
                        break;
                    case Limb.Left_Calf:
                        limbAng = CalcLimbAngular(playerInfo, ImiGesture.leftFootIndex, ImiGesture.leftKneeIndex);
                        break;
                    case Limb.Left_Calf_Ankle:
                        limbAng = CalcLimbAngular(playerInfo, ImiGesture.leftAnkleIndex, ImiGesture.leftKneeIndex);
                        break;
                    case Limb.Left_Thigh:
                        limbAng = CalcLimbAngular(playerInfo, ImiGesture.leftKneeIndex, ImiGesture.leftHipIndex);
                        break;
                    case Limb.Right_Forearm:
                        limbAng = CalcLimbAngular(playerInfo, ImiGesture.rightHandIndex, ImiGesture.rightElbowIndex);
                        break;
                    case Limb.Right_Forearm_Wrist:
                        limbAng = CalcLimbAngular(playerInfo, ImiGesture.rightWristIndex, ImiGesture.rightElbowIndex);
                        break;
                    case Limb.Right_Postbrachium:
                        limbAng = CalcLimbAngular(playerInfo, ImiGesture.rightElbowIndex, ImiGesture.rightShoulderIndex);
                        break;
                    case Limb.Right_Calf:
                        limbAng = CalcLimbAngular(playerInfo, ImiGesture.rightFootIndex, ImiGesture.rightKneeIndex);
                        break;
                    case Limb.Right_Calf_Ankle:
                        limbAng = CalcLimbAngular(playerInfo, ImiGesture.rightAnkleIndex, ImiGesture.rightKneeIndex);
                        break;
                    case Limb.Right_Thigh:
                        limbAng = CalcLimbAngular(playerInfo, ImiGesture.rightKneeIndex, ImiGesture.rightHipIndex);
                        break;
                    case Limb.Spine:
                        limbAng = CalcLimbAngular(playerInfo, ImiGesture.spineIndex, ImiGesture.shoulderCenterIndex);
                        break;
                }
                playerInfo.SetPlayerLimbsAngular(j, limbAng);
            }
        }

        private Vector3 CalcLimbAngular(ImiPlayerInfo playerInfo, int joint1, int joint2)
        {
            //Vector3[] playerJointsPos = playerInfo.GetPlayerJointsPos();
            //bool[] playerJointsTracked = playerInfo.GetPlayerJointsTracked();
            ImiSkeleton[] skeletons = playerInfo.GetSkeletons();

            if (skeletons[joint1].isTracked && skeletons[joint2].isTracked)
            {
                float kYX = (skeletons[joint1].position.y - skeletons[joint2].position.y) /
                            (skeletons[joint1].position.x - skeletons[joint2].position.x);
                float angYX = (float)(Math.Atan(kYX) * (180 / Math.PI));

                float kXZ = (skeletons[joint1].position.x - skeletons[joint2].position.x) /
                            (skeletons[joint1].position.z - skeletons[joint2].position.z);
                float angXZ = (float)(Math.Atan(kXZ) * (180 / Math.PI));

                float kYZ = (skeletons[joint1].position.y - skeletons[joint2].position.y) /
                            (skeletons[joint1].position.z - skeletons[joint2].position.z);
                float angYZ = (float)(Math.Atan(kYZ) * (180 / Math.PI));

                return new Vector3(angYX, angYZ, angXZ);
            }
            return new Vector3(-1.0f, -1.0f, -1.0f);
        }


        //计算关节点四元数.
        private void CalcJointsOri(ImiPlayerInfo playerInfo)
        {
            Vector3 vx;
            Vector3 vy;
            Vector3 vz;
            ImiSkeleton[] skeletons = playerInfo.GetSkeletons();

            // NUI_SKELETON_POSITION_HIP_CENTER
            if (skeletons[ImiGesture.hipCenterIndex].isTracked && skeletons[ImiGesture.spineIndex].isTracked)
            {
                vy = GetPositionBetweenIndices(skeletons, ImiGesture.hipCenterIndex, ImiGesture.spineIndex);
                vx = GetPositionBetweenIndices(skeletons, ImiGesture.leftHipIndex, ImiGesture.rightHipIndex);
                Quaternion quat = Utils.MakeQuaternionFromYX(vx, vy);

                // make a correction of about 40 degrees back to the front
                //quat *= Quaternion.Euler(-40, 0, 0);
                skeletons[ImiGesture.hipCenterIndex].orientation = quat;
            }

            // NUI_SKELETON_POSITION_SPINE
            if (skeletons[ImiGesture.spineIndex].isTracked && skeletons[ImiGesture.shoulderCenterIndex].isTracked)
            {
                vy = GetPositionBetweenIndices(skeletons, ImiGesture.spineIndex, ImiGesture.shoulderCenterIndex);
                vx = GetPositionBetweenIndices(skeletons, ImiGesture.leftShoulderIndex, ImiGesture.rightShoulderIndex);
                Quaternion quat = Utils.MakeQuaternionFromYX(vx, vy);
                skeletons[ImiGesture.spineIndex].orientation = quat;
            }

            // NUI_SKELETON_POSITION_SHOULDER_CENTER
            if (skeletons[ImiGesture.shoulderCenterIndex].isTracked && skeletons[ImiGesture.headIndex].isTracked)
            {
                vy = GetPositionBetweenIndices(skeletons, ImiGesture.shoulderCenterIndex, ImiGesture.headIndex);
                vx = GetPositionBetweenIndices(skeletons, ImiGesture.leftShoulderIndex, ImiGesture.rightShoulderIndex);
                Quaternion quat = Utils.MakeQuaternionFromYX(vx, vy);
                skeletons[ImiGesture.shoulderCenterIndex].orientation = quat;
            }

            // NUI_SKELETON_POSITION_HEAD
            if (skeletons[ImiGesture.shoulderCenterIndex].isTracked && skeletons[ImiGesture.headIndex].isTracked)
            {
                vy = GetPositionBetweenIndices(skeletons, ImiGesture.shoulderCenterIndex, ImiGesture.headIndex);
                vx = GetPositionBetweenIndices(skeletons, ImiGesture.leftShoulderIndex, ImiGesture.rightShoulderIndex);
                Quaternion quat = Utils.MakeQuaternionFromYX(vx, vy);
                skeletons[ImiGesture.headIndex].orientation = quat;
            }

            // NUI_SKELETON_POSITION_SHOULDER_LEFT
            if (skeletons[ImiGesture.leftShoulderIndex].isTracked && skeletons[ImiGesture.leftElbowIndex].isTracked)
            {
                vx = -GetPositionBetweenIndices(skeletons, ImiGesture.leftShoulderIndex, ImiGesture.leftElbowIndex);
                vy = GetPositionBetweenIndices(skeletons, ImiGesture.leftElbowIndex, ImiGesture.leftWristIndex);
                Quaternion quat = Utils.MakeQuaternionFromXY(vx, vy);
                skeletons[ImiGesture.leftShoulderIndex].orientation = quat;
            }

            // NUI_SKELETON_POSITION_ELBOW_LEFT
            if (skeletons[ImiGesture.leftElbowIndex].isTracked && skeletons[ImiGesture.leftWristIndex].isTracked)
            {
                vx = -GetPositionBetweenIndices(skeletons, ImiGesture.leftElbowIndex, ImiGesture.leftWristIndex);
                vy = -GetPositionBetweenIndices(skeletons, ImiGesture.leftShoulderIndex, ImiGesture.leftElbowIndex);
                Quaternion quat = Utils.MakeQuaternionFromXY(vx, vy);
                skeletons[ImiGesture.leftElbowIndex].orientation = quat;
            }

            // NUI_SKELETON_POSITION_WRIST_LEFT
            if (skeletons[ImiGesture.leftWristIndex].isTracked && skeletons[ImiGesture.leftHandIndex].isTracked)
            {
                vx = -GetPositionBetweenIndices(skeletons, ImiGesture.leftWristIndex, ImiGesture.leftHandIndex);
                Quaternion quat = Utils.MakeQuaternionFromX(vx, false);
                skeletons[ImiGesture.leftWristIndex].orientation = quat;
            }

            // NUI_SKELETON_POSITION_HAND_LEFT:
            if (skeletons[ImiGesture.leftWristIndex].isTracked && skeletons[ImiGesture.leftHandIndex].isTracked)
            {
                vx = -GetPositionBetweenIndices(skeletons, ImiGesture.leftWristIndex, ImiGesture.leftHandIndex);
                Quaternion quat = Utils.MakeQuaternionFromX(vx, false);
                skeletons[ImiGesture.leftHandIndex].orientation = quat;
            }

            // NUI_SKELETON_POSITION_SHOULDER_RIGHT
            if (skeletons[ImiGesture.rightShoulderIndex].isTracked && skeletons[ImiGesture.rightElbowIndex].isTracked)
            {
                vx = GetPositionBetweenIndices(skeletons, ImiGesture.rightShoulderIndex, ImiGesture.rightElbowIndex);
                vy = GetPositionBetweenIndices(skeletons, ImiGesture.rightElbowIndex, ImiGesture.rightWristIndex);
                Quaternion quat = Utils.MakeQuaternionFromXY(vx, vy);
                skeletons[ImiGesture.rightShoulderIndex].orientation = quat;
            }

            // NUI_SKELETON_POSITION_ELBOW_RIGHT
            if (skeletons[ImiGesture.rightElbowIndex].isTracked && skeletons[ImiGesture.rightWristIndex].isTracked)
            {
                vx = GetPositionBetweenIndices(skeletons, ImiGesture.rightElbowIndex, ImiGesture.rightWristIndex);
                vy = -GetPositionBetweenIndices(skeletons, ImiGesture.rightShoulderIndex, ImiGesture.rightElbowIndex);
                Quaternion quat = Utils.MakeQuaternionFromXY(vx, vy);
                skeletons[ImiGesture.rightElbowIndex].orientation = quat;
            }

            // NUI_SKELETON_POSITION_WRIST_RIGHT
            if (skeletons[ImiGesture.rightWristIndex].isTracked && skeletons[ImiGesture.rightHandIndex].isTracked)
            {
                vx = GetPositionBetweenIndices(skeletons, ImiGesture.rightWristIndex, ImiGesture.rightHandIndex);
                Quaternion quat = Utils.MakeQuaternionFromX(vx, true);
                skeletons[ImiGesture.rightWristIndex].orientation = quat;
            }

            // NUI_SKELETON_POSITION_HAND_RIGHT
            if (skeletons[ImiGesture.rightWristIndex].isTracked && skeletons[ImiGesture.rightHandIndex].isTracked)
            {
                vx = GetPositionBetweenIndices(skeletons, ImiGesture.rightWristIndex, ImiGesture.rightHandIndex);
                Quaternion quat = Utils.MakeQuaternionFromX(vx, true);
                skeletons[ImiGesture.rightHandIndex].orientation = quat;
            }

            // NUI_SKELETON_POSITION_HIP_LEFT
            if (skeletons[ImiGesture.leftHipIndex].isTracked && skeletons[ImiGesture.leftKneeIndex].isTracked)
            {
                vy = GetPositionBetweenIndices(skeletons, ImiGesture.leftKneeIndex, ImiGesture.leftHipIndex);
                vx = GetPositionBetweenIndices(skeletons, ImiGesture.leftHipIndex, ImiGesture.rightHipIndex);
                Quaternion quat = Utils.MakeQuaternionFromYX(vx, vy);
                skeletons[ImiGesture.leftHipIndex].orientation = quat;
            }

            // NUI_SKELETON_POSITION_KNEE_LEFT
            if (skeletons[ImiGesture.leftKneeIndex].isTracked && skeletons[ImiGesture.leftAnkleIndex].isTracked)
            {
                vy = -GetPositionBetweenIndices(skeletons, ImiGesture.leftKneeIndex, ImiGesture.leftAnkleIndex);
                vz = -GetPositionBetweenIndices(skeletons, ImiGesture.leftAnkleIndex, ImiGesture.leftFootIndex);
                Quaternion quat = Utils.MakeQuaternionFromYZ(vy, vz);
                skeletons[ImiGesture.leftKneeIndex].orientation = quat;
            }

            // NUI_SKELETON_POSITION_ANKLE_LEFT
            if (skeletons[ImiGesture.leftAnkleIndex].isTracked && skeletons[ImiGesture.leftFootIndex].isTracked)
            {
                vz = GetPositionBetweenIndices(skeletons, ImiGesture.leftFootIndex, ImiGesture.leftAnkleIndex);
                vy = -GetPositionBetweenIndices(skeletons, ImiGesture.leftKneeIndex, ImiGesture.leftAnkleIndex);
                Quaternion quat = Utils.MakeQuaternionFromYZ(vy, vz);
                skeletons[ImiGesture.leftAnkleIndex].orientation = quat;
            }

            // NUI_SKELETON_POSITION_FOOT_LEFT
            if (skeletons[ImiGesture.leftAnkleIndex].isTracked && skeletons[ImiGesture.leftFootIndex].isTracked)
            {
                vz = GetPositionBetweenIndices(skeletons, ImiGesture.leftFootIndex, ImiGesture.leftAnkleIndex);
                vy = -GetPositionBetweenIndices(skeletons, ImiGesture.leftKneeIndex, ImiGesture.leftAnkleIndex);
                Quaternion quat = Utils.MakeQuaternionFromYZ(vy, vz);
                skeletons[ImiGesture.leftFootIndex].orientation = quat;
            }

            // NUI_SKELETON_POSITION_HIP_RIGHT
            if (skeletons[ImiGesture.rightHipIndex].isTracked && skeletons[ImiGesture.rightKneeIndex].isTracked)
            {
                vy = GetPositionBetweenIndices(skeletons, ImiGesture.rightKneeIndex, ImiGesture.rightHipIndex);
                vx = GetPositionBetweenIndices(skeletons, ImiGesture.leftHipIndex, ImiGesture.rightHipIndex);
                Quaternion quat = Utils.MakeQuaternionFromYX(vx, vy);
                skeletons[ImiGesture.rightHipIndex].orientation = quat;
            }

            // NUI_SKELETON_POSITION_KNEE_RIGHT
            if (skeletons[ImiGesture.rightKneeIndex].isTracked && skeletons[ImiGesture.rightAnkleIndex].isTracked)
            {
                vy = -GetPositionBetweenIndices(skeletons, ImiGesture.rightKneeIndex, ImiGesture.rightAnkleIndex);
                vz = -GetPositionBetweenIndices(skeletons, ImiGesture.rightAnkleIndex, ImiGesture.rightFootIndex);
                Quaternion quat = Utils.MakeQuaternionFromYZ(vy, vz);
                skeletons[ImiGesture.rightKneeIndex].orientation = quat;
            }

            // NUI_SKELETON_POSITION_ANKLE_RIGHT
            if (skeletons[ImiGesture.rightAnkleIndex].isTracked && skeletons[ImiGesture.rightFootIndex].isTracked)
            {
                vz = GetPositionBetweenIndices(skeletons, ImiGesture.rightFootIndex, ImiGesture.rightAnkleIndex);
                vy = -GetPositionBetweenIndices(skeletons, ImiGesture.rightKneeIndex, ImiGesture.rightAnkleIndex);
                Quaternion quat = Utils.MakeQuaternionFromYZ(vy, vz);
                skeletons[ImiGesture.rightAnkleIndex].orientation = quat;
            }

            // NUI_SKELETON_POSITION_FOOT_RIGHT
            if (skeletons[ImiGesture.rightAnkleIndex].isTracked && skeletons[ImiGesture.rightFootIndex].isTracked)
            {
                vz = GetPositionBetweenIndices(skeletons, ImiGesture.rightFootIndex, ImiGesture.rightAnkleIndex);
                vy = -GetPositionBetweenIndices(skeletons, ImiGesture.rightKneeIndex, ImiGesture.rightAnkleIndex);
                Quaternion quat = Utils.MakeQuaternionFromYZ(vy, vz);
                skeletons[ImiGesture.rightFootIndex].orientation = quat;
            }
        }

        private Vector3 GetPositionBetweenIndices(ImiSkeleton[] skeletons, int jointIndexOne, int jointIndexTwo)
        {
            Vector3 pVecOne = skeletons[(int)jointIndexOne].position;
            Vector3 pVecTwo = skeletons[(int)jointIndexTwo].position;

            return pVecTwo - pVecOne;
        }

        public ImiPlayerInfo GetPlayerInfo(int userIndex)
        {
            if (playerInfos.ContainsKey(userIndex))
            {
                return playerInfos[userIndex];
            }else
            {
                return null;
            }
        }


        //计算关节点速度.
        private void CalcJointsSpeed(ImiPlayerInfo playerInfo)
        {
            ImiSkeleton[] skeletons = playerInfo.GetSkeletons();
            int userIndex = playerInfo.GetUserId();

            for (int j = 0; j < TOTAL_JOINTS; j++)
            {
                if (skeletons[j].isTracked)
                {
                    if (distanceDatas[userIndex, j].Count < 2)
                    {
                        distanceDatas[userIndex, j].Add(skeletons[j].position);
                        timeDatas[userIndex, j].Add(DateTime.Now);
                    }
                    else
                    {
                        TimeSpan ts = timeDatas[userIndex, j][1] - timeDatas[userIndex, j][0];

                        Vector4 tempData;
                        if (preferences.isNeedVecSpeed)
                        {
                            tempData = VecVector4((distanceDatas[userIndex, j][1] - distanceDatas[userIndex, j][0]), (float)(ts.TotalMilliseconds / 100));
                        }
                        else
                        {
                            tempData = AbsVector4((distanceDatas[userIndex, j][1] - distanceDatas[userIndex, j][0]), (float)(ts.TotalMilliseconds / 100));
                        }


                        distanceDatas[userIndex, j].RemoveAt(0);
                        distanceDatas[userIndex, j].Add(skeletons[j].position);
                        timeDatas[userIndex, j].RemoveAt(0);
                        timeDatas[userIndex, j].Add(DateTime.Now);

                        if (speedDatas[userIndex, j].Count < speedFrameLimit)
                        {
                            speedDatas[userIndex, j].Add(tempData);
                        }
                        else
                        {
                            Vector4 allDistanceTime = CalcDisTime(speedDatas[userIndex, j]);

                            playerInfos[userIndex].GetSkeletons()[j].speed = new Vector3(allDistanceTime.x / allDistanceTime.w, allDistanceTime.y / allDistanceTime.w, allDistanceTime.z / allDistanceTime.w);

                            speedDatas[userIndex, j].RemoveAt(0);
                            speedDatas[userIndex, j].Add(tempData);
                        }
                    }
                }
            }
        }

        private Vector4 AbsVector4(Vector3 v3, float seconds)
        {
            return new Vector4(Math.Abs(v3.x), Math.Abs(v3.y), Math.Abs(v3.z), seconds);
        }

        private Vector4 VecVector4(Vector3 v3, float seconds)
        {
            return new Vector4(v3.x, v3.y, v3.z, seconds);
        }

        private Vector4 CalcDisTime(List<Vector4> datas)
        {
            float distanceX = 0;
            float distanceY = 0;
            float distanceZ = 0;
            float time = 0;
            for (int i = 0; i < datas.Count; i++)
            {
                distanceX += datas[i].x;
                distanceY += datas[i].y;
                distanceZ += datas[i].z;
                time += datas[i].w;
            }
            return new Vector4(distanceX, distanceY, distanceZ, time);
        }


        public Texture2D GetImiTexture(ImiTexture2D.Type type)
        {
            if (!imiTextures.ContainsKey(type))
            {
                imiTextures.Add(type, ImiTexture2DFactory.CreateTexture(type));
            }
            return imiTextures[type].Texture;
        }


        public Texture2D GetColorTexture()
        {

#if UNITY_ANDROID && !UNITY_EDITOR
            if (imiManager.IsPerformance)
            {
                return allUserColorTexImproved;
            }
            else
            {

                return clrTex;
                return GetImiTexture(ImiTexture2D.Type.COLOR);
            }
#else
            return clrTex;
            return GetImiTexture(ImiTexture2D.Type.COLOR);
#endif
        }

        public Texture2D GetDepthTexture()
        {
            return GetImiTexture(ImiTexture2D.Type.DEPTH);
        }

        public Texture2D GetUserDepthTexture()
        {
            return usersLblTex;
            return GetImiTexture(ImiTexture2D.Type.USER_DEPTH);
        }

        public Texture2D GetMainUserDepthTexture()
        {
            return GetImiTexture(ImiTexture2D.Type.MAIN_USER_DEPTH);
        }

        public Texture2D GetSubUserDepthTexture()
        {
            return GetImiTexture(ImiTexture2D.Type.SUB_USER_DEPTH);
        }

        private bool checkResolution()
        {
            if (preferences.colorResolution.type != preferences.depthResolution.type)
            {
                Debug.LogError("Cannot Cutout Color Texuture Because Depth Resolution is not equal to Color Resolution");
                return false;
            }
            return true;

        }

        public Texture2D GetMainUserColorTexture()
        {
            return checkResolution() ? null : GetImiTexture(ImiTexture2D.Type.MAIN_USER_COLOR);
        }

        public Texture2D GetSubUserColorTexture()
        {
            return checkResolution() ? null : GetImiTexture(ImiTexture2D.Type.SUB_USER_COLOR);
        }

        public bool IsDeviceOpened()
        {
            return deviceOpenState == DeviceOpenState.OPENED;
        }

        /// <summary>
        /// 设置控制玩家ID.
        /// </summary>
        /// <param name="index"></param>
        public void SetControlPlayerId(int userId)
        {
            isControlPlayerAssigned = true;
            if (this.controlPlayerId != userId && controlPlayerChangerObserver != null)
            {
                controlPlayerChangerObserver(this.controlPlayerId, userId);
            }
            this.controlPlayerId = userId;
        }

        public int GetControlPlayerId()
        {
            return this.controlPlayerId;
        }

        public int GetSubPlayerId()
        {
            return this.subControlPlayerId;
        }

        public ImiResolution GetColorResolution()
        {
            return preferences.colorResolution;
        }

        public ImiResolution GetDepthResolution()
        {
            return preferences.depthResolution;
        }

        public void SetControlPlayerChangeListener(ImiManager.OnControlPlayerChanged observer)
        {
            this.controlPlayerChangerObserver = observer;
        }

        public void SetDepthDataProcessor(ImiManager.DepthDataProcessor processor)
        {
            this.depthDataProcessor = processor;
        }

        public void SetColorDataProcessor(ImiManager.ColorDataProcessor processor)
        {
            this.colorDataProcessor = processor;
        }

        public void SetRawDataProcessor(ImiManager.RawDataProcessor processor)
        {
            this.rawDataProcessor = processor;
        }

    }

}
